/*
 * 改编自ARMxDSP消息队列实现ARMxDSP回环测试
 */

#if defined(__SANDBLASTER__)
#include "armxdsp.h"
#include "armxdsp-impl.h"

static int armxdsp_use_outq(int dsp, void* mq_addr);
static int armxdsp_use_inq(int dsp, void* mq_addr);

#ifdef __SANDBLASTER__
#define ACCESS_LOCK_BYTE(lock,i) ((lock)[(i)^3])
#else
#define ACCESS_LOCK_BYTE(lock,i) ((lock)[i])
#define __sb_barrier()
#endif

#define MQLOCK_SELF  1 /* 1 means DSP */
#define MQLOCK_OTHER 2 /* 2 means ARM */

typedef struct armxdsp_isr_info {
    int installed;
    armxdsp_receive_handler_t handler;
    void* param;
} armxdsp_isr_info_t;

#ifdef __SANDBLASTER__
static void send_interrupt_signal(int dsp) {
    /* 写VIC直接绕过HAB中的DSP中断逻辑 */
    *(volatile unsigned*)0x88005018 = 1 << dsp;
}

//假设SB_MODEL_SB3500，所以没有地址转换
#if 1 // __SBMODELNUM__ == SB_MODEL_SB3500
#define MAKE_DSP_ADDR(x) ((void*)(x))
#else
#define MAKE_DSP_ADDR(x) \
    ((void*)(((uint32_t)(x))|((-(((uint32_t)(x))&0x70000000))&0x80000000)))
#endif

#else // ! __SANDBLASTER__
static void send_interrupt_signal(int dsp) {
}

#endif // __SANDBLASTER__

#ifndef BPB_LIST_ADDRESS
#define BPB_LIST_ADDRESS (0x40800000-4096)
#endif

#define BPB_TAG_DSP     0x4250424C /* DSP bootloader parameters */
#define BPB_TAG_ARM     0x41425042 /* ARM OS-less bootloader parameters */
#define BPB_TAG_LINUX   0x42504203 /* Linux boot parameters */
#define BPB_TAG_DSP_MEM 0x4D616368 /* DSP memory regions info */

armxdsp_mq_pointers_t* find_mq_pointers(void) {
    uint32_t* p =  (uint32_t*)MAKE_DSP_ADDR(BPB_LIST_ADDRESS);
    while (*p != 0) {
        switch (*p) {
            case ARMXDSP_MQ_POINTERS_TAG:
                return (armxdsp_mq_pointers_t*)p;
            case BPB_TAG_DSP_MEM:
                /* 计算DSP存储区域信息块大小;
                    该块由7个字头组成，每个区域加上3个字 */
                p = (uint32_t*)((char*)p
                    + 7*sizeof(uint32_t) /* 头 */
                    + p[5]*3*sizeof(uint32_t)); /* 区域 */
                break;
            case BPB_TAG_DSP:
            case BPB_TAG_ARM:
            case BPB_TAG_LINUX:
                /* 块大小（以字节为单位）存储在第二个字中 */
                if (p[1] != 0) {
                    p = (uint32_t*)((char*)p + p[1]);
                    continue;
                }
            default:
                return NULL;
        }
    }
    return NULL;
}

int armxdsp_open_queues(void) {
    int i;
    armxdsp_mq_pointers_t* p = find_mq_pointers();
    if (p == NULL) {
        return ARMXDSP_EINIT;
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        armxdsp_mq_t* q = p->dsp_to_arm[i];
        if (q != NULL) {
            if (armxdsp_use_outq(i, MAKE_DSP_ADDR(q))) {
                return ARMXDSP_EINIT;
            }
        }
    }
    for (i = 0; i < ARMXDSP_DSP_COUNT; ++i) {
        armxdsp_mq_t* q = p->arm_to_dsp[i];
        if (q != NULL) {
            if (armxdsp_use_inq(i, MAKE_DSP_ADDR(q))) {
                return ARMXDSP_EINIT;
            }
        }
    }
    return 0;
}

#undef MAKE_DSP_ADDR

#ifndef ACCESS_LOCK_BYTE
#define ACCESS_LOCK_BYTE(lock,i) ((lock)[i])
#endif

static void mqlock_lock(volatile uint8_t* lock) {
    ACCESS_LOCK_BYTE(lock, MQLOCK_SELF) = 1;
    ACCESS_LOCK_BYTE(lock, 0) = MQLOCK_OTHER;
    while (ACCESS_LOCK_BYTE(lock, MQLOCK_OTHER) != 0
            && ACCESS_LOCK_BYTE(lock, 0) == MQLOCK_OTHER) {
        POLL_DELAY();
    }
}

static void mqlock_unlock(volatile uint8_t* lock) {
    ACCESS_LOCK_BYTE(lock, MQLOCK_SELF) = 0;
}

static int armxdsp_use_outq(int dsp, void* mq_addr) {
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
    out_queues[dsp] = (armxdsp_mq_t*)mq_addr;
    return 0;
}

static int armxdsp_use_inq(int dsp, void* mq_addr) {
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
    in_queues[dsp] = (armxdsp_mq_t*)mq_addr;
    return 0;
}

int armxdsp_send_begin_dsp(armxdsp_sender_t* sender, int dsp, int slot,
        uint32_t size) {
    for (;;) {
        int status = armxdsp_send_begin_nb_dsp(sender, dsp, slot, size);
        if (status != ARMXDSP_EAGAIN) {
            return status;
        }
        BLOCK_DELAY();
    }
}

int armxdsp_send_begin_nb_dsp(armxdsp_sender_t* sender, int dsp, int slot_,
        uint32_t size) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t used_size;
    uint32_t above_wpos;
    uint32_t* data;
    uint32_t slot = slot_;

#if ARMXDSP_CHECK_ARGS != 0
    if (sender == NULL) {
        return ARMXDSP_EINVAL;
    }
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
#endif
    q = out_queues[dsp];
#if ARMXDSP_CHECK_ARGS != 0
    if (q == NULL) {
        return ARMXDSP_EDSP;
    }
    if (slot < 0 || slot >= q->slot_count) {
        return ARMXDSP_ESLOT;
    }
#endif
    s = &q->slots[slot];
    used_size = MESSAGE_SIZE(size);
#if ARMXDSP_CHECK_ARGS != 0
    if (used_size > s->size) {
        return ARMXDSP_ESIZE;
    }
#endif
    above_wpos = s->size - s->wpos;
    if (used_size > above_wpos) {
        ARMXDSP_ASSERT(used_size <= s->size);
        if (used_size + above_wpos > GET_SLOT_FREE_SPACE(s)) {
            if (s->used_size != 0) {
                return ARMXDSP_EAGAIN;
            }
            mqlock_lock(q->lock);
            s->wpos = 0;
            s->rpos = 0;
            mqlock_unlock(q->lock);
        } else {
            *GET_SLOT_DATA(q,s,s->wpos) = (uint32_t)(-(int32_t)above_wpos);
            used_size += above_wpos;
        }
        data = GET_SLOT_DATA(q,s,0);
    } else {
        if (used_size > GET_SLOT_FREE_SPACE(s)) {
            return ARMXDSP_EAGAIN;
        }
        data = GET_SLOT_DATA(q,s,s->wpos);
    }
    *data++ = size;
    sender->dsp = dsp;
    sender->slot = slot;
    sender->size = size;
    sender->data = data;
    sender->used_size = used_size;
    return 0;
}

void armxdsp_send_end_dsp(armxdsp_sender_t* sender) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t ready;
    uint32_t wpos;
    ARMXDSP_ASSERT(sender != NULL);
    q = out_queues[sender->dsp];
    s = &q->slots[sender->slot];
    mqlock_lock(q->lock);
    s->used_size += sender->used_size;
    wpos = s->wpos + sender->used_size;
    if (wpos >= s->size) {
        /* wrap around wpos */
        wpos -= s->size;
        ARMXDSP_ASSERT(wpos < s->size);
    }
    s->wpos = wpos;
    ready = q->msg_ready;
    q->msg_ready = ready | (1 << sender->slot);
    if (ready == 0) {
        send_interrupt_signal(sender->dsp);
    }
    mqlock_unlock(q->lock);
}

int armxdsp_receive_begin_dsp(armxdsp_receiver_t* receiver, int dsp, uint32_t slotmask) {
    for (;;) {
        int status = armxdsp_receive_begin_nb_dsp(receiver, dsp, slotmask);
        if (status != ARMXDSP_EAGAIN) {
            return status;
        }
        BLOCK_DELAY();
    }
}

int armxdsp_receive_begin_nb_dsp(armxdsp_receiver_t* receiver, int dsp, uint32_t slotmask) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t* data;
    uint32_t ready;
    uint32_t size;
    uint32_t used_size;
    int ready_slot;
#if ARMXDSP_CHECK_ARGS != 0
    if (receiver == NULL) {
        return ARMXDSP_EINVAL;
    }
    if (dsp < 0 || dsp >= ARMXDSP_DSP_COUNT) {
        return ARMXDSP_EDSP;
    }
#endif
    q = in_queues[dsp];
#if ARMXDSP_CHECK_ARGS != 0
    if (q == NULL) {
        return ARMXDSP_EDSP;
    }
#endif
    ready = q->msg_ready & slotmask;
    if (ready == 0) {
        return ARMXDSP_EAGAIN;
    }
    ready_slot = 0;
    while ((ready & 1) == 0 && ready_slot < (int)q->slot_count) {
        ++ready_slot;
        ready >>= 1;
    }
    s = &q->slots[ready_slot];
    used_size = 0;
    data = GET_SLOT_DATA(q,s,s->rpos);
    if ((int32_t)*data < 0) {
        uint32_t rpos = s->rpos - *data;
        used_size -= *data;
        if (rpos >= s->size) {
            /* wrap around rpos */
            rpos -= s->size;
            ARMXDSP_ASSERT(rpos < s->size);
        }
        data = GET_SLOT_DATA(q,s,rpos);
    }
    size = *data++;
    used_size += MESSAGE_SIZE(size);
    receiver->dsp = dsp;
    receiver->slot = ready_slot;
    receiver->size = size;
    receiver->data = data;
    receiver->used_size = used_size;
    return 0;
}

void armxdsp_receive_end_dsp(armxdsp_receiver_t* receiver) {
    armxdsp_mq_t* q;
    armxdsp_mq_slot_t* s;
    uint32_t rpos;
    ARMXDSP_ASSERT(receiver != NULL);
    q = in_queues[receiver->dsp];
    s = &q->slots[receiver->slot];
    mqlock_lock(q->lock);
    s->used_size -= receiver->used_size;
    rpos = s->rpos + receiver->used_size;
    if (rpos >= s->size) {
        /* 绕圈子 */
        rpos -= s->size;
        ARMXDSP_ASSERT(rpos < s->size);
    }
    s->rpos = rpos;
    if (s->used_size == 0) {
        q->msg_ready &= ~(1 << receiver->slot);
    }
    mqlock_unlock(q->lock);
}

#endif
